/*
 * terrainFoot.cpp
 *
 *  Created on: 5 mai 2010
 *      Author: raph
 */
#include <cstdlib>
#include <iostream>
#include <string.h>
#include <unistd.h>
#include <cv.h>
#include <highgui.h>
#include <cxcore.h>
#include <stdlib.h>
#include <math.h>
#include <cvaux.h>
#include <stdio.h>
#include "terrainFoot.h"

#define LONGUEUR_TERRAIN 120
#define LARGEUR_TERRAIN 90
#define SEUILLAGE_BALLE_VIOLET 75
#define NOMBRE_POINTS_CALIBRAGE 17

/************************************************/
/******** Variables globales au fichiers ********/
/************************************************/

// Ameliorations possible : Faire une classe avec des attributs
CvPoint balleTerrain;
CvPoint balleEcran;
CvPoint centreButEcran;
CvMat* homographyMat = cvCreateMat(3, 3, CV_32FC1);
IplImage* src = NULL;
IplImage* img = 0;
int iSaisie = 0;
CvPoint coordCalibrage[NOMBRE_POINTS_CALIBRAGE];
bool afficherText = true;


/************************************************/
/********************* Main *********************/
/************************************************/

int main(void)
    {
    CvCapture* capture = NULL;

    if (NULL == (capture = cvCaptureFromCAM(-1)))
	{
	printf("\nError on cvCaptureFromCAM");
	return -1;
	}

    firstCapture(capture);
    init();

    calibrageTerrain();

    CvMat* matPointEcran = cvCreateMat(3, 1, CV_32FC1);
    CvMat* matPointTerrainReel = cvCreateMat(3, 1, CV_32FC1);

    int key;
    for (;;)
	{
	if (NULL == (src = cvQueryFrame(capture)))
	    {
	    printf("\nError on cvQueryFrame");
	    break;
	    }

	CvSeq *circles = findCircles(src);

	if (circles->total >= 1)
	    {
	    float *p = (float*) cvGetSeqElem(circles, 0); // indice 0 => premier cercle
	    balleEcran = cvPoint(cvRound(p[0]), cvRound(p[1]));

	    // Ajout de la balle dans la matrice
	    cvmSet(matPointEcran, 0, 0, balleEcran.x);
	    cvmSet(matPointEcran, 1, 0, balleEcran.y);
	    cvmSet(matPointEcran, 2, 0, 1);

	    // Passage ecran -> reel, [px] -> [m]
	    cvMatMul(homographyMat, matPointEcran, matPointTerrainReel);

	    // On repasse en 2d, l'attribut en Z doit valoir 1
	    balleTerrain.x = cvmGet(matPointTerrainReel, 0, 0) / cvmGet(
		    matPointTerrainReel, 2, 0);
	    balleTerrain.y = cvmGet(matPointTerrainReel, 1, 0) / cvmGet(
		    matPointTerrainReel, 2, 0);

	    CvPoint destinationBalleReelle = cvPoint(LARGEUR_TERRAIN / 2, 0);
	    double d = distance(balleTerrain, destinationBalleReelle);
	    drawDistance(src, d);
	    //	    afficherText = false;
	    }

	// acutalisation de l'image capture
	cvShowImage("Capture", src);

	key = cvWaitKey(10);
	if (key == 0x1b)
	    break;
	}

    cvReleaseCapture(&capture);
    cvDestroyWindow("Capture");

    return 0;
    }

/************************************************/
/******************* Fonctions ******************/
/************************************************/

void init()
    {
    cvNamedWindow("Capture", CV_WINDOW_AUTOSIZE);
    cvMoveWindow("Capture", 50, 50);
    cvShowImage("Capture", src);
    // Set up the callback
    cvSetMouseCallback("Capture", mouseCallback, (void*) (src));
    }

void firstCapture(CvCapture* capture)
    {
    int key;
    int i = 1;

    // Necessaire d'attendre 3 requete pour avoir une image correcte
    while (i <= 3)
	{
	if (NULL == (src = cvQueryFrame(capture)))
	    {
	    printf("\nError on cvQueryFrame");
	    break;
	    }

	key = cvWaitKey(10);
	i++;
	}
    }

void mouseCallback(int event, int x, int y, int flags, void* param)
    {
    switch (event)
	{
    case CV_EVENT_LBUTTONDOWN:
	if (iSaisie < NOMBRE_POINTS_CALIBRAGE)
	    {
	    coordCalibrage[iSaisie] = cvPoint(x, y);
	    iSaisie++;
	    }
	else
	    {
	    iSaisie = 100;
	    }
	afficherText = true;
	break;
	}
    }

void calibrageTerrain()
    {
    // Machine d'etat	1..16	: saisie des coordonnees (calibrage du terrain
    //			17	: calcul de la matrice homographique en fonction des coordonnees saisies
    //
    // Note:	Cette fonction n'est pas tres elegante, il aurait ete preferable de faire du developpement objet avec des classes
    //		s'occupant de ces traitements. On aurait egalement pu eviter l'attente active (while i <= 17).
    //
    // - afficherTexte varie (true/false) de façon a n'afficher en console les messages de saisies qu'une seule fois
    // - l'incrementation de iSaisie se fait dans mouseCallback, apres chaque click de l'utilisateur
    while (iSaisie <= NOMBRE_POINTS_CALIBRAGE)
	{
	if (afficherText == true)
	    {
	    switch (iSaisie)
		{
	    case 0:
		std::cout << "*** Debut du calibrage ***\n";
		std::cout << "* Cliquez sur le coin sup droit\n";
		afficherText = false;
		break;
	    case 1:
		std::cout << "* Cliquez sur le coin sup gauche\n";
		afficherText = false;
		break;
	    case 2:
		std::cout << "* Cliquez sur le coin inf droit\n";
		afficherText = false;
		break;
	    case 3:
		std::cout << "* Cliquez sur le coin inf gauche\n";
		afficherText = false;
		break;
	    case 4:
		std::cout << "* Cliquez sur le milieu droit\n";
		afficherText = false;
		break;
	    case 5:
		std::cout << "* Cliquez sur le milieu gauche\n";
		afficherText = false;
		break;
	    case 6:
		std::cout << "* Cliquez sur le milieu centre\n";
		afficherText = false;
		break;
	    case 7:
		std::cout << "* Cliquez sur le penalty sup\n";
		afficherText = false;
		break;
	    case 8:
		std::cout << "* Cliquez sur le poteau sup droit\n";
		afficherText = false;
		break;
	    case 9:
		std::cout << "* Cliquez sur le poteau sup gauche\n";
		afficherText = false;
		break;
	    case 10:
		std::cout << "* Cliquez sur le coin de la surface sup droit\n";
		afficherText = false;
		break;
	    case 11:
		std::cout << "* Cliquez sur le coin de la surface sup gauche\n";
		afficherText = false;
		break;
	    case 12:
		std::cout << "* Cliquez sur le point de penalty inf\n";
		afficherText = false;
		break;
	    case 13:
		std::cout << "* Cliquez sur le poteau inf droite\n";
		afficherText = false;
		break;
	    case 14:
		std::cout << "* Cliquez sur le poteau inf gauche\n";
		afficherText = false;
		break;
	    case 15:
		std::cout << "* Cliquez sur le coin de la surface inf droit\n";
		afficherText = false;
		break;
	    case 16:
		std::cout << "* Cliquez sur le coin de la surface inf gauche\n";
		afficherText = false;
		break;

	    case 17:
		afficherText = false;

		findHomographyPerso(coordCalibrage, homographyMat);

		std::cout << "*** Fin du calibrage ***\n";
		iSaisie = 100; // force la sortie de la boucle
		break;
		}
	    }

	// Necessaire d'attendre l'appuie d'une touche pour que l'image s'affiche
	if (cvWaitKey(15) == 27)
	    break;
	}
    }

double distance(CvPoint p1, CvPoint p2)
    {
    double add = pow(abs(p1.x - p2.x), 2) + pow(abs(p1.y - p2.y), 2);
    return sqrt(add);
    }

CvSeq* findCircles(IplImage *image)
    {
    IplImage *imageNiveauGris = cvCreateImage(cvGetSize(image), 8, 1);
    IplImage *imageSeuillee = cvCreateImage(cvGetSize(image), 8, 1);
    int seuillage = SEUILLAGE_BALLE_VIOLET;

    // Transformation en niveau de gris
    cvCvtColor(image, imageNiveauGris, CV_BGR2GRAY);
    cvSmooth(imageNiveauGris, imageNiveauGris, CV_GAUSSIAN, 9, 9);

    // Seuillage de l'image en niveau de gris
    cvThreshold(imageNiveauGris, imageSeuillee, seuillage, 255,
	    CV_THRESH_BINARY);
    cvSmooth(imageSeuillee, imageSeuillee, CV_GAUSSIAN, 9, 9);

    // Detection des cercles. retour dans une sequence
    CvMemStorage *storage = cvCreateMemStorage(0);
    return cvHoughCircles(imageSeuillee, storage, CV_HOUGH_GRADIENT, 2,
	    imageSeuillee->height / 2, 10, 13);
    }

void drawDistance(IplImage *& img, double d)
    {
    // Dessine un point sur la balle
    int rayonBalle = 2;
    cvCircle(img, balleEcran, rayonBalle, CV_RGB(255, 0, 0), 2, 8, 0);
    centreButEcran = cvPoint((coordCalibrage[8].x + (coordCalibrage[9].x
	    - coordCalibrage[8].x) / 2.0), (coordCalibrage[8].y
	    + (coordCalibrage[9].y - coordCalibrage[8].y) / 2.0));

    // Dessine la ligne de la distance balle - but
    cvLine(img, balleEcran, centreButEcran, CV_RGB(255, 0, 0), 2, CV_AA);

    // Ecrit la distance reelle calculee
    CvFont font;
    cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0.0, 2);
    char *strDistance = new char[256];
    sprintf(strDistance, "Disance : %.1lf m", d);
    CvPoint coordStartText = cvPoint(balleEcran.x+10,balleEcran.y+10);
    std::cout << "coordStartText : " << coordStartText.x << std::endl;
    std::cout << "src : " << src->width << std::endl;
    if(coordStartText.x > src->width/2 )
	{
	std::cout << "if" << std::endl;
	coordStartText.x -= src->width/2;
	}
    cvPutText(img, strDistance, coordStartText,&font, cvScalar(127));
    }

void findHomographyPerso(CvPoint * pointsEcran, CvMat* matHomography)
    {
        CvMat* matPointsTerrainReel = cvCreateMat(NOMBRE_POINTS_CALIBRAGE, 2,
	    CV_32FC1);
        CvMat *matPointsEcran = cvCreateMat(NOMBRE_POINTS_CALIBRAGE, 2, CV_32FC1);
        for (int i = 0; i < NOMBRE_POINTS_CALIBRAGE; i++)
	{
	cvmSet(matPointsEcran, i, 0, (double) pointsEcran[i].x);
	cvmSet(matPointsEcran, i, 1, (double) pointsEcran[i].y);
	}
        initMatriceTerrainReel(matPointsTerrainReel);

    cvFindHomography(matPointsEcran, matPointsTerrainReel, matHomography);

    cvReleaseMat(&matPointsEcran);
    cvReleaseMat(&matPointsTerrainReel);
    }

void initMatriceTerrainReel(CvMat *& matPointsTerrainReel)
{
    cvmSet(matPointsTerrainReel, 0, 0, (double)(0));
    cvmSet(matPointsTerrainReel, 0, 1, (double)(0));
    cvmSet(matPointsTerrainReel, 1, 0, (double) LARGEUR_TERRAIN);
    cvmSet(matPointsTerrainReel, 1, 1, (double)(0));
    cvmSet(matPointsTerrainReel, 2, 0, (double)(0));
    cvmSet(matPointsTerrainReel, 2, 1, (double) LONGUEUR_TERRAIN);
    cvmSet(matPointsTerrainReel, 3, 0, (double) LARGEUR_TERRAIN);
    cvmSet(matPointsTerrainReel, 3, 1, (double) LONGUEUR_TERRAIN);
    cvmSet(matPointsTerrainReel, 4, 0, (double)(0));
    cvmSet(matPointsTerrainReel, 4, 1, (double) LONGUEUR_TERRAIN / 2);
    cvmSet(matPointsTerrainReel, 5, 0, (double) LARGEUR_TERRAIN);
    cvmSet(matPointsTerrainReel, 5, 1, (double) LONGUEUR_TERRAIN / 2);
    cvmSet(matPointsTerrainReel, 6, 0, (double) LARGEUR_TERRAIN / 2);
    cvmSet(matPointsTerrainReel, 6, 1, (double) LONGUEUR_TERRAIN / 2);
    cvmSet(matPointsTerrainReel, 7, 0, (double) LARGEUR_TERRAIN / 2);
    cvmSet(matPointsTerrainReel, 7, 1, (double)(11));
    cvmSet(matPointsTerrainReel, 8, 0, (double) (LARGEUR_TERRAIN / 2) - 3.65);
    cvmSet(matPointsTerrainReel, 8, 1, (double)(0));
    cvmSet(matPointsTerrainReel, 9, 0, (double) (LARGEUR_TERRAIN / 2) + 3.65);
    cvmSet(matPointsTerrainReel, 9, 1, (double)(0));
    cvmSet(matPointsTerrainReel, 10, 0, (double) (LARGEUR_TERRAIN / 2) - 3.65
	    - 16.5);
    cvmSet(matPointsTerrainReel, 10, 1, (double)(16.5));
    cvmSet(matPointsTerrainReel, 11, 0, (double) (LARGEUR_TERRAIN / 2) + 3.65
	    + 16.5);
    cvmSet(matPointsTerrainReel, 11, 1, (double)(16.5));
    cvmSet(matPointsTerrainReel, 12, 0, (double) LARGEUR_TERRAIN / 2);
    cvmSet(matPointsTerrainReel, 12, 1, (double) LONGUEUR_TERRAIN - 11);
    cvmSet(matPointsTerrainReel, 13, 0, (double) (LARGEUR_TERRAIN / 2) - 3.65);
    cvmSet(matPointsTerrainReel, 13, 1, (double) LONGUEUR_TERRAIN);
    cvmSet(matPointsTerrainReel, 14, 0, (double) (LARGEUR_TERRAIN / 2) + 3.65);
    cvmSet(matPointsTerrainReel, 14, 1, (double) LONGUEUR_TERRAIN);
    cvmSet(matPointsTerrainReel, 15, 0, (double) (LARGEUR_TERRAIN / 2) - 3.65
	    - 16.5);
    cvmSet(matPointsTerrainReel, 15, 1, (double) LONGUEUR_TERRAIN - 16.5);
    cvmSet(matPointsTerrainReel, 16, 0, (double) (LARGEUR_TERRAIN / 2) + 3.65
	    + 16.5);
    cvmSet(matPointsTerrainReel, 16, 1, (double) LONGUEUR_TERRAIN - 16.5);
}
